/*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*\
|          Copyright (C) 2013 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |
                                                                                
     This program is free software: you can redistribute it and/or modify       
     it under the terms of the GNU General Public License version 2 as          
     published by the Free Software Foundation.                                 
                                                                                
     This program is distributed in the hope that it will be useful,            
     but WITHOUT ANY WARRANTY; without even the implied warranty of             
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              
     GNU General Public License for more details.                               
                                                                                
     You should have received a copy of the GNU General Public License          
     along with this program.  If not, see <http://www.gnu.org/licenses/>.      
                                                                                
            Jack Elston                                                         
|           elstonj@colorado.edu                                               |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/
#include "can_usb.h"

//---------------------------------------------------------------------------
CANUSB::CANUSB(String name) : pThread(name)
{
	state=UNKNOWN;
	m_UcanHandle = USBCAN_INVALID_HANDLE;
}

bool CANUSB::init(ShmStruct * shmStructPtr)
{
  bool retval = pThread::init(shmStructPtr);
	
	initCANStruct();
	initCAN();

  return retval;
}

// --------------------------------------------------------------------------
CANUSB::~CANUSB()
{
	if( m_UcanHandle != USBCAN_INVALID_HANDLE )
		closeCAN();
}

// --------------------------------------------------------------------------
void CANUSB::closeCAN(void)
{
	UcanDeinitCan (m_UcanHandle);
	UcanDeinitHardware (m_UcanHandle);
	m_UcanHandle = USBCAN_INVALID_HANDLE;
	setStatus("Device Closed");
}

// --------------------------------------------------------------------------
void CANUSB::initCANStruct()
{
	union  idUnion
	{
		uint16_t sData[2];
		uint32_t dData;
	} idu;

	short int adrs[] = {0x1F0D,
		0x1F0F,
		0x1F04,
		0x0105,
		0x0106,
		0x0103,
		0x0005,
		0x0006,
		0x0200,
		0x0201,
		0x0202,
		0x0203,
		0x0000,
		0x0001,
		0x0002,
		0x0003,
		0x0004,
		0X0005,
		0X0023,
		0X0024,
		0x0007,
		0X0008,
		0xFFFF};

	// Add the Broadcast address for tx Messages;
	idu.sData[0] = 65535;

	for (int i=PITOT;i<SER0;i++)
	{
		idu.sData[1] = adrs[i];
		aCanMsg[i].m_dwID = idu.dData;
		aCanMsg[i].m_bFF =  USBCAN_MSG_FF_EXT;
		for (int j =0; j<8 ;j++)
		{
			aCanMsg[i].m_bData[j]=0;
		}
		aCanMsg[i].m_dwTime =0.0f;
	}

	for (int i=SER0;i<SIMI;i++)
	{
		idu.sData[1] = adrs[i];
		aCanMsg[i].m_dwID = idu.dData;
		aCanMsg[i].m_bFF =  USBCAN_MSG_FF_EXT;
		for (int j =0; j<8 ;j++)
		{
			aCanMsg[i].m_bData[j]=0;
		}
		aCanMsg[i].m_dwTime =0.0f;
		aCanMsg[i].m_bDLC = 8;
	}

	// Simulator Reset needs FFFF
	aCanMsg[SIMI].m_dwID = 0x0008FFFF;

	// Configure the length of the messages
	aCanMsg[PITOT].m_bDLC = 8;
	aCanMsg[BARO].m_bDLC = 8;
	aCanMsg[TEMP].m_bDLC = 8;
	aCanMsg[GYRO].m_bDLC = 7;
	aCanMsg[ACCEL].m_bDLC = 7;
	aCanMsg[TIME].m_bDLC = 6;
	aCanMsg[MAG].m_bDLC = 6;
	aCanMsg[ENG].m_bDLC = 8;
	aCanMsg[GPST].m_bDLC = 8;
	aCanMsg[GPSP].m_bDLC = 8;
	aCanMsg[GPSA].m_bDLC = 4;
	aCanMsg[GPSV].m_bDLC = 6;
	aCanMsg[SIMI].m_bDLC = 0;
}

// --------------------------------------------------------------------------
void CANUSB::initCAN(void)
{
	WORD                wBaudrate;
	DWORD               dwAMR, dwACR;
	UCANRET             ret = USBCAN_ERR;
	DWORD               dwUcanLibVersion;
	tUcanHardwareInfo   HwInfo;

	int errsv;
	int num_tries = 0;

	switch (state) {
		case UNKNOWN:
			mutex_unlock();
			shmPtr->usbcan_status.apAddress = 0;
			mutex_lock();

			m_UcanHandle  = USBCAN_INVALID_HANDLE;

			swPeriod=1;
			state = CHECK_LIB;
		case CHECK_LIB:
			dwUcanLibVersion = UcanGetVersionEx (kVerTypeUserLib);
			if(dwUcanLibVersion == 0) {
				setStatus("Library not Installed");
				return;
			}

			char tmpStr[80];
			sprintf (tmpStr,"* Library version = %d.%02d r%d\n",
					USBCAN_MAJOR_VER (dwUcanLibVersion),
					USBCAN_MINOR_VER (dwUcanLibVersion),
					USBCAN_RELEASE_VER (dwUcanLibVersion));
			mutex_unlock();
			shmPtr->usbcan_status.lib = tmpStr;
			mutex_lock();

			// set initialisation parameters
			bDeviceNr = USBCAN_ANY_MODULE;  // init any module which is found at first
			wBaudrate = USBCAN_BAUD_1MBit;  // baud rate on CAN bus
			dwAMR     = USBCAN_AMR_ALL;     // acceptance filter for receiving CAN message
			dwACR     = USBCAN_ACR_ALL;     // ...

			//printf ("using device number %d%s\n", bDeviceNr, (bDeviceNr == USBCAN_ANY_MODULE) ? "(any)" : "");
			//printf ("CAN baud rate register 0x%04X%s\n", wBaudrate, (wBaudrate == USBCAN_BAUD_1MBit) ? "(1MBit/sec)" : "");
			usleep (2000 * 1000);   // waiting 2sec
			state = INIT_HW;
		case INIT_HW:
			ret = UcanInitHardware (&m_UcanHandle, bDeviceNr, NULL);
			if (ret != USBCAN_SUCCESSFUL)
			{
				errsv = errno;
				setError(errsv);
				setStatus("Can't Init Hardware");
				return;
			}
			setStatus("Hardware Initialized");
			state = READ_INFO;
		case READ_INFO:
			ret = UcanGetHardwareInfo (m_UcanHandle, &HwInfo);
			if (ret == USBCAN_SUCCESSFUL) {
				setStatus("Read HW Info");
				//printf ("we got USB-CANmodul with device number %d\n", HwInfo.m_bDeviceNr);
			}
			state = INIT_CAN_BUS;
		case INIT_CAN_BUS:
			// initialize CAN interface of the device (red LED switches off)
			ret = UcanInitCan (m_UcanHandle, (BYTE) (wBaudrate >> 8), (BYTE) (wBaudrate & 0xFF), dwAMR, dwACR);
			if (ret != USBCAN_SUCCESSFUL)
			{
				errsv = errno;
				setError(errsv);
				setStatus("Can't Init CAN Bus");
				return;
			}
			setStatus("Initialized");
			state = FIND_PICCOLO;
		case FIND_PICCOLO:
			num_tries = 0;
			while (readCANBus () != USBCAN_SUCCESSFUL) {
				num_tries ++;
				if(num_tries > 10)
				{
					setStatus("Piccolo Not Attached");
					return;
				}
			}
			// set CAN message headers for Piccolo ID
			for (int i=SER0;i<=CONF;i++)
				((uint16_t *)(&(aCanMsg[i].m_dwID)))[0] = shmPtr->usbcan_status.apAddress;

			setStatus("Piccolo Attached");
			state = SIM_RESET;
		case SIM_RESET:
			ret = UcanWriteCanMsg (m_UcanHandle, &aCanMsg[SIMI]);
			if (ret != USBCAN_SUCCESSFUL)
			{
				errsv = errno;
				setError(errsv);
				setStatus("Unable to Reset Sim");
			}
			setStatus("OK");
			state = INITIALIZED;
		case INITIALIZED:
			break;
	}
}

// --------------------------------------------------------------------------
void CANUSB::update(void)
{
	if(state != INITIALIZED) {initCAN(); return;}

	UCANRET idx = readCANBus();
	system_t* status;
	bool read = false;
	bool send = false;

	switch (idx) {
		case CONF:
			status = &(shmPtr->packet_status.conf);
			read = true;
			break;
		case SER0: // more than likely this channel is used
			status = &(shmPtr->packet_status.servo);
			read = true;
			break;
		default:
			break;
	}

	if(read) {
			mutex_unlock();
			shmPtr->usbcan_status.rx ++;
			if(status->ready)
				shmPtr->usbcan_status.dropped ++;
			copyFromCAN(idx);
			status->ready = true;
			mutex_lock();
	}

	if(shmPtr->packet_status.air.ready) {
		idx = PITOT; send = true;
		status = &(shmPtr->packet_status.air); }
	if(shmPtr->packet_status.imu.ready) {
		idx = GYRO; send = true;
		status = &(shmPtr->packet_status.imu); }
	if(shmPtr->packet_status.engine.ready) {
		idx = ENG; send = true;
		status = &(shmPtr->packet_status.engine); }
	if(shmPtr->packet_status.gps.ready) {
		idx = GPST; send = true;
		status = &(shmPtr->packet_status.gps); }

	if(send) {
		copyToCAN(idx);
		if(writeCANBus(idx) == USBCAN_SUCCESSFUL) {
			mutex_unlock();

			shmPtr->usbcan_status.tx ++; // counting messages rather than bytes;
			currentStamp.stamp();
			status->rate = SEC2MICRO / (currentStamp - status->time);
			status->time.stamp();

			status->ready = false;
			mutex_lock();
		}
	}

}

//---------------------------------------------------------------------------
UCANRET CANUSB::readCANBus (void) {
	UCANRET ret = UcanReadCanMsg (m_UcanHandle, &aCanMsg[RCV]);
	if (USBCAN_CHECK_VALID_RXCANMSG (ret))
	{
		if (USBCAN_CHECK_WARNING (ret))
		{
			setStatus("Warning on Read");
			return ret;
		}

		return(processCANMsg());
	}
	else if (USBCAN_CHECK_ERROR (ret))
	{
		int errsv = errno;
		setError(errsv);
		setStatus("Error on Read");
		return ret;
	}
	return ret;
}

//---------------------------------------------------------------------------
UCANRET CANUSB::writeCANBus (UCANRET idx) {
	// transmit CAN message to CAN bus
	UCANRET ret = USBCAN_ERR;

	UCANRET start, end;
	switch (idx) {
		case PITOT:
		case BARO:
		case TEMP:
			start = PITOT; end = TEMP; break;
		case GYRO:
		case ACCEL:
		case TIME:
			start = GYRO; end = TIME; break;
		case MAG:
			return ret;  // not implemented
		case ENG:
			start = ENG; end = ENG; break;
		case GPST:
		case GPSP:
		case GPSA:
		case GPSV:
			start = GPST; end = GPSV; break;
		default:
			return ret;
			break;
	}

	for(int i = start; i <= end; i++) {
		ret = UcanWriteCanMsg (m_UcanHandle,  &aCanMsg[i]);
		usleep(500);
		if (USBCAN_CHECK_TX_OK (ret))
		{
			if (USBCAN_CHECK_WARNING (ret))
			{
				setStatus("Warning on Write");
				return ret;
			}
		}
		else if (USBCAN_CHECK_ERROR (ret))
		{
			int errsv = errno;
			setError(errsv);
			setStatus("Error on Write");
			return ret;
		}
	}
	return ret;
}

//---------------------------------------------------------------------------
void CANUSB::copyToCAN (UCANRET idx) {
	uint8_t * dataPtr;

	switch (idx) {
		case PITOT:
		case BARO:
		case TEMP:
			dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.air));
			for (int ii=0;ii<8 ;ii++)
			{
				aCanMsg[PITOT].m_bData[ii] = *(dataPtr + ii);
				aCanMsg[BARO].m_bData[ii] = *(dataPtr + 8 + ii);
				aCanMsg[TEMP].m_bData[ii] = *(dataPtr +16 + ii);
			}
			break;
		case GYRO:
		case ACCEL:
		case TIME:
			dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.imu));
			for (int ii=0;ii<6 ;ii++)
			{
				aCanMsg[GYRO].m_bData[ii] = *(dataPtr + ii);
				aCanMsg[ACCEL].m_bData[ii] = *(dataPtr + ii+6);
			}

			aCanMsg[TIME].m_bData[5] = shmPtr->piccolo_rx.imu.sequence;

			// Put timing data NOT DOCUMENTED noticed on mesg
			aCanMsg[GYRO].m_bData[6] = aCanMsg[TIME].m_bData[5];
			aCanMsg[ACCEL].m_bData[6] = aCanMsg[TIME].m_bData[5];
			break;
		case MAG:
			break;
		case ENG:
			dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.engine));
			memcpy(aCanMsg[ENG].m_bData,dataPtr,sizeof(engineStruct_t));
			break;
		case GPST:
		case GPSP:
		case GPSA:
		case GPSV:
			dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.gps.month));
			for (int ii=0;ii<8 ;ii++)
			{
				aCanMsg[GPST].m_bData[ii] = *(dataPtr + ii);
				aCanMsg[GPSP].m_bData[ii] = *(dataPtr + ii + 8);
			}

			dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.gps.height));
			for (int ii=0;ii<4 ;ii++)
			{
				aCanMsg[GPSA].m_bData[ii] = *(dataPtr + ii);
			}

			dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.gps.velocity));
			for (int ii=0;ii<6 ;ii++)
			{
				aCanMsg[GPSV].m_bData[ii] = *(dataPtr + ii);
			}
			break;
	}
}

//---------------------------------------------------------------------------
void CANUSB::copyFromCAN (UCANRET idx) {
	//assumes you have unlocked the mutex
	uint8_t* ptr;
	switch (idx) {
		case SER0:
		case SER1:
		case SER2:
		case SER3:
		case SER4:
		case SER5:
		case SER6:
		case SER7:
			ptr = (uint8_t*)(&(shmPtr->piccolo_tx.servos));
			for(int idx=SER0;idx<=SER7;idx++) {
				memcpy(ptr,aCanMsg[idx].m_bData,aCanMsg[idx].m_bDLC);
				ptr += aCanMsg[idx].m_bDLC;
			}
			break;
		case CONF:
			ptr = (uint8_t*)(&(shmPtr->piccolo_tx.config));
			memcpy(ptr,aCanMsg[CONF].m_bData,aCanMsg[CONF].m_bDLC);
			break;
	}
}

//---------------------------------------------------------------------------
int CANUSB::processCANMsg (void)
{
	DWORD idx_msg = -1;

	if(shmPtr->usbcan_status.apAddress == 0) {
		mutex_unlock();
		shmPtr->usbcan_status.apAddress = uint16_t(aCanMsg[RCV].m_dwID);
		mutex_lock();
		return USBCAN_SUCCESSFUL;
	}

	if (aCanMsg[RCV].m_dwID == aCanMsg[SER0].m_dwID){
		idx_msg = SER0;
	}
	else {
		if (aCanMsg[RCV].m_dwID == aCanMsg[SER1].m_dwID) {
			idx_msg = SER1;
		}
		else {
			if (aCanMsg[RCV].m_dwID == aCanMsg[SER2].m_dwID) {
				idx_msg = SER2;
			}
			else {
				if (aCanMsg[RCV].m_dwID == aCanMsg[SER3].m_dwID) {
					idx_msg = SER3;
				}
				else {
					if (aCanMsg[RCV].m_dwID == aCanMsg[SER4].m_dwID) {
						idx_msg = SER4;
					}
					else {
						if (aCanMsg[RCV].m_dwID == aCanMsg[SER5].m_dwID) {
							idx_msg = SER5;
						}
						else {
							if (aCanMsg[RCV].m_dwID == aCanMsg[SER6].m_dwID) {
								idx_msg = SER6;
							}
							else {
								if (aCanMsg[RCV].m_dwID == aCanMsg[SER7].m_dwID) {
									idx_msg = SER7;
								}
								else {
									if (aCanMsg[RCV].m_dwID == aCanMsg[CONF].m_dwID){
										idx_msg = CONF;
									}
								}
							}
						}
					}
				}
			}
		}
	}
	if (idx_msg != -1)
	{
		for (int ii = 0; ii < 8 ; ii++)
		{
			aCanMsg[idx_msg].m_bData[ii] = aCanMsg[RCV].m_bData[ii];
		}
	}
	return idx_msg;
}

//---------------------------------------------------------------------------
void CANUSB::setStatus(const char* status) {
	mutex_unlock();
	shmPtr->usbcan_status.state = status;
	mutex_lock();
}

//---------------------------------------------------------------------------
void CANUSB::setError(int errsv) {
	mutex_unlock();
	shmPtr->usbcan_status.error = errsv;
	mutex_lock();
}
